from pb.ros.geometry_msgs import Pose_pb2
import proxy


node = proxy.Connector('tcp://172.22.56.12:15555') #在709的电脑上使用该IP
#node = proxy.Connector('tcp://12.12.12.204:15555') #在集群的电脑上使用该IP
publish_cmd_pos = node.Advertise('FK','M300_0', '/FK/M300_0/set_pos', Pose_pb2.Pose) #发给动力学模型的控制输入

x1,y1,z1,x,y,z,w = -300,200,200,0,0,0,0

def cmd_pos():
    cmd_pos = Pose_pb2.Pose()
    cmd_pos.position.x = x1
    cmd_pos.position.y = y1
    cmd_pos.position.z = z1
    cmd_pos.orientation.x = x
    cmd_pos.orientation.y = y
    cmd_pos.orientation.z = z
    cmd_pos.orientation.w = w
    publish_cmd_pos.Publish(cmd_pos)


cmd_pos()
